clear
% clc
close all

plotThings =0;

data.boot.soleLength = 30e-2;
data.binding.model = 'bulldog';
data.binding.nTubes = 1;

data.binding.springs = [1,0.5]*.0254*4;  %vector of spring lengths in series
data.binding.springType = {'red','blue'}; %vector of spring types in series

% data.binding.springs=2*4*.0254;
% data.binding.springType = {'red'};

data.binding.pivotPosition = 7;
data.binding.preload = 0.5*.0254;

a0 = [0.;0]; % initial toe and bellows angle
options.a = 0; %set dummy options set
options.maxIts = 1e3;
options.relax =0.5; %relaxation for newton solver, should be between
% 0 and 1, 1 means that its going to jump to the solution to the linear
% system, 0 means that its not going to move, somewhere between is how far
% in the direction of the linear solution it will jump, for big scary
% nonlinearities, using a value of 1 may often overshoot and put you in
% some region of badness, if things get ugly, try to bring it down, and
% then bump up the max its (since it will take longer to get there...)

%figure out initial heel lift force 
[m0] = basicBoot(a0,0,data);

Fh0 = m0(1)/data.boot.soleLength;

FhL = abs(Fh0); % lower limit of heel pull force 
FhU = 200; % upper limit of heel pull force
dFh = 10; %heel pull force increment


Fhs = FhL:dFh:FhU;



if plotThings
    hh = figure;
    axis equal
    grid on
    disp('adjust boot visualization plot for viewing during sim, then hit enter')
    pause
end

tic

alphaN = a0;
aNs = zeros(2,length(Fhs));
Fh_torque = zeros(1,length(Fhs));
rotation = zeros(1,length(Fhs));
for i = 1:length(Fhs)
    
    [alphaN,sigM] = dampedNewton(@basicBoot,alphaN,options,Fhs(i),data);
    
%     Fhs(i)
    
     [~,~,~,Fh_torque(i),rotation(i)] = basicBoot(alphaN,Fhs(i),data);
     
     aNs(:,i) = alphaN;
     
     if any(isnan(alphaN))
         Fh_torque = Fh_torque(1:end-1);
         rotation =rotation(1:end-1);
         aNs = aNs(:,1:end-1);
         break;
     end
     
     if plotThings
         clf
        axis equal
        grid on
         plotBindingPosition(data,alphaN,hh,'b')
         tstring = ['[FhL,alpha-toe (deg),alpha-bellows (deg)]=[',num2str(Fhs(i),'%2.3g'),' , ',...
             num2str(alphaN(1)*180/pi,'%2.3g'),' , ',num2str(alphaN(2)*180/pi,'%2.3g'),']'];
         title(tstring)
     
     
        pause(.01)
        
     end
end

toc

rotation = rotation*180/pi;

figure
plot(rotation,-Fh_torque)
xlabel('Heel rotation angle')
ylabel('Heel lift torque')
grid on